import numpy as np
from mpl_toolkits import mplot3d
import matplotlib.pyplot as plt


# 三次曲线
def hermite(Pi, Pj, Di, Dj):
    Mh =np.array([[ 2,-2, 1, 1],
                 [-3, 3,-2,-1],
                 [ 0, 0, 1, 0],
                 [ 1, 0, 0, 0]])
    pts =np.array([Pi, Pj, Di, Dj])
    u_vec = lambda u:np.array([u**3, u**2, u, 1])
    curve = np.zeros((1001, 3))
    # 创建区间：
    for u in range(1001):
        curve[u] = u_vec(u*0.001) @ Mh @ pts
    return curve

# 简单的窗口绘图
if __name__ == '__main__':
    # 创建画布
    plt.rcParams["font.sans-serif"]=["SimHei"]  # 设置字体
    plt.rcParams["axes.unicode_minus"]=False    # “-”负号的显示
    fig = plt.figure()                          # 空的画布
    ax = fig.add_subplot(111, projection='3d')  # 创建3d绘图区域
    ax.tick_params(labelsize=8)                 # 刻度字体

    # 绘图
    # 坐标点
    Pi = [   0,    0,    0]
    Pj = [1000, 1000, 1000]
    Di = [   0, 3000, 2800]
    Dj = [4600,    0, 4800]
    curve = hermite(Pi, Pj, Di, Dj)
    ax.plot(curve[:,0], curve[:,1], curve[:,2])
    ax.set_title('Hermite 曲线')

    # 参考线
    def axs(xx=(-1,1),yx=(-1,1),zx=(-1,1)): 
        color = ['r', 'g', 'b']
        for i,t in  enumerate((xx, yx, zx)):
            xs = [(0,0), (0,0), (0,0)]
            f = True if t[0]<0 and t[1]>0 else False
            if f:
                xs[i] = (t[0], 0)
                ax.plot(xs[0], xs[1], xs[2], linestyle=':', color=color[i])
                xs[i] = (0, t[1])
                ax.plot(xs[0], xs[1], xs[2], linestyle='-', color=color[i])
            else: 
                xs[i] = (t[0], t[1])
                ax.plot(xs[0], xs[1], xs[2], linestyle='-', color=color[i])
    axs((0,500),(0,500),zx=(0,500))
    ax.grid(b=None) # 隐藏网格

    # 显示窗口
    plt.show()